package com.googlecode.gunncs.sensor;

import java.awt.Color;

import javaclient3.RangerInterface;
import javaclient3.structures.PlayerPose3d;

import com.googlecode.grtframework.core.GRTLoggedProcess;
import com.googlecode.gunncs.graph.TimedLinePlot;

/**
 * A RangeFinder publishes RangeFinder events
 * 
 * @author ajc
 * 
 */
public class NoisyRangefinder extends GRTLoggedProcess {

	private static final Color[] colors = new Color[] { Color.red,
			Color.orange, Color.yellow, Color.GREEN, Color.blue };

	private final RangerInterface ri;
	private double[] sonarValues;
	private PlayerPose3d[] poses;
	TimedLinePlot ranges;

	public NoisyRangefinder(RangerInterface ri) {
		super("ranger");
		this.ri = ri;
		// poses = ri.getGeom().getPoses();
		ranges = new TimedLinePlot("ranges", 100);
		ranges.setYRangingFixed(0, 10);
		for (int i = 0; i < 5; i++) {
			ranges.addData("" + i, colors[i], "?");
		}
	}

	public void run() {
		running = true;
		while (running) {
			if (!suspended) {
				if (ri.isDataReady()) {
					sonarValues = ri.getData().getRanges();
					for (int i = 0; i < 5; i++) {
						ranges.addPoint("" + i, sonarValues[i]);
					}
				}
			}

			try {
				Thread.sleep(50);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}

		}
	}

}
